import xml.etree.ElementTree as ET

def get_total_mass_from_urdf(urdf_file_path):
    # 加载 URDF 文件
    tree = ET.parse(urdf_file_path)
    root = tree.getroot()

    total_mass = 0.0

    # 遍历所有 link 标签
    for link in root.findall('.//link'):
        inertial = link.find('inertial')
        if inertial is not None:
            mass_element = inertial.find('mass')
            if mass_element is not None:
                mass_value = float(mass_element.attrib['value'])
                total_mass += mass_value

    return total_mass

# 使用示例
urdf_file_path = '/home/xjz/catkin_ws/src/unitree_ros/robots/aliengo_description/urdf/aliengo.urdf'  # 替换为你的 URDF 文件路径

total_mass = get_total_mass_from_urdf(urdf_file_path)
if total_mass > 0.0:
    print(f"The total mass of the robot is: {total_mass}")
else:
    print("Total mass not found or could not be calculated.")